/******************************************************************************/
/*!

\file   Matrix.cpp
\author M.J. Gardner
\date   12/2/2011
\brief
      templated nXm matrix math library. See Matrix.hpp for details



*/
/******************************************************************************/
#include <algorithm>  //min
#include <cmath>      //sqrt, pow
#include <cassert>

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Matrix member functions start here

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*
  all manner of constructors
*/

template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T>::Matrix(void)
{
  ZeroMtx(*this);
}

template<unsigned int R, unsigned int C, typename T>
template<unsigned int A, unsigned int B>
Matrix<R, C, T>::Matrix(const Matrix<A, B, T> &rhs)
{
  CopyElemHelper(rhs);
}

template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T>::Matrix(const std::array<T, C*R> &rhs)
{
  for(unsigned i = 0; i < R*C; ++i)
  {
    (*this)[i] = rhs[i];
  }
}

template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T>::Matrix(const std::array<std::array<T, C>, R> &rows)
{
  for(unsigned i = 0; i < R; ++i)
  {
    for(unsigned j = 0; j < C; ++j)
    {
      (*this)(i, j) = rows[i][j];
    }
  }
}

//template<unsigned int R, unsigned int C, typename T>
//Matrix<R, C, T>::Matrix(const std::array<std::array<T, R>, C> &cols)
//{
//  for(unsigned i = 0; i < C; ++i)
//  {
//    for(unsigned j = 0; j < R; ++j)
//    {
//      (*this)(j, i) = cols[i][j];
//    }
//  }
//}

template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T>::Matrix(const typename std::array<T, C*R>::iterator &begin_it,
                        const typename std::array<T, C*R>::iterator &end_it)
{
  typename std::array<T, C*R>::iterator it = begin_it;
  MTX_IT m_it = begin();
  for(; it != end_it; ++it)
  {
    *m_it = *it;
    ++m_it;
  }
}

template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T>::Matrix(const typename std::array<std::array<T, C>, R>::iterator &r_begin,
          const typename std::array<std::array<T, C>, R>::iterator &r_end)
{
  unsigned row = 0;
  for(typename std::array<std::array<T, C>, R>::iterator outer_it = r_begin;
      outer_it != r_end; ++outer_it)
  {
    ROW_IT r_it = row_begin(row);
    for(typename std::array<T, C>::iterator it = outer_it->begin();
        it != outer_it->end(); ++it)
    {
      *r_it = *it;
      ++r_it;
    }
    ++row;
  }
}

//template<unsigned int R, unsigned int C, typename T>
//Matrix<R, C, T>::Matrix(const typename std::array<std::array<T, R>, C>::iterator &c_begin,
//                        const typename std::array<std::array<T, R>, C>::iterator &c_end)
//{
//  unsigned col = 0;
//  for(typename std::array<std::array<T, R>, C>::iterator outer_it = c_begin;
//      outer_it != c_end; ++outer_it)
//  {
//    COL_IT c_it = col_begin(col);
//    for(typename std::array<T, R>::iterator it = outer_it->begin();
//        it != outer_it->end(); ++it)
//    {
//      *c_it = *it;
//      ++c_it;
//    }
//    ++col;
//  }
//}

template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T>::Matrix(const std::initializer_list<T> &rhs)
{
  ZeroMtx(*this);

  MTX_IT m_it = begin();
  for(typename std::initializer_list<T>::iterator it = rhs.begin();
      it != rhs.end() && m_it != end(); ++it)
  {
    *m_it = *it;
    ++m_it;
  }
}

template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T>::Matrix(const std::initializer_list<std::initializer_list<T>> &rhs)
{
  ZeroMtx(*this);
  unsigned row = 0;
  for(typename std::initializer_list<std::initializer_list<T>>::iterator outer = rhs.begin();
      outer != rhs.end() && row < R; ++outer)
  {
    ROW_IT r_it = row_begin(row);
    for(typename std::initializer_list<T>::iterator inner = outer->begin();
        inner != outer->end() && r_it != row_end(row); ++inner)
    {
      *r_it = *inner;
      ++r_it;
    }

    ++row;
    if(row >= R)
      break;
  }
}

template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T>::Matrix(T *rhs)
{
  for(unsigned i = 0; i < R*C; ++i)
    (*this)[i] = rhs[i];
}

template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T>::Matrix(T **rhs)
{
  for(unsigned i = 0; i < R; ++i)
  {
    for(unsigned j = 0; j < C; ++j)
      (*this)(i, j) = rhs[i][j];
  }
}




/******************************************************************************/
/*!
  \brief
    2-dimensional subscript for matrices

  \param  row
    the row number of the element we want

  \param column
    the column number of the element we want

  \return
    reference to the element at the position specified
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
T &Matrix<R, C, T>::operator()(const unsigned int row, const unsigned int column)
{
  return (*this)[(row * C) + column];
}
/******************************************************************************/
/*!
  \brief
    2-dimensional subscript for matrices (const version)

  \param  row
    the row number of the element we want

  \param column
    the column number of the element we want

  \return
    const reference to the element at the position specified
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
const T &Matrix<R, C, T>::operator()(const unsigned int row, const unsigned int column) const
{
  return (*this)[(row * C) + column];
}


/******************************************************************************/
/*!
  \brief
    1-dimensional subscript for matrices

  \param  index
    index into the matrix, as if it were a 1-dimensional array.
    convenient for vectors.

  \return
    reference to the element at the given index
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
T &Matrix<R, C, T>::operator[](const unsigned int index)
{
  return _elems.at(index);
}
/******************************************************************************/
/*!
  \brief
    1-dimensional subscript for matrices (const version)

  \param  index
    index into the matrix, as if it were a 1-dimensional array.
    convenient for vectors.

  \return
    const reference to the element at the given index
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
const T &Matrix<R, C, T>::operator[](const unsigned int index) const
{
  return _elems.at(index);
}


/******************************************************************************/
/*!
  \brief
    get "general" iterator to the beginning of a matrix

  \return
    iterator to the beginning, which can be used to iterate over the entire matrix
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
MTX_IT Matrix<R, C, T>::begin(void)
{
  return MTX_IT(_elems.begin());
}
/******************************************************************************/
/*!
  \brief
    get "general" iterator to the end of a matrix

  \return
    iterator to the end of the entire matrix
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
MTX_IT Matrix<R, C, T>::end(void)
{
  return MTX_IT(_elems.end());
}


/******************************************************************************/
/*!
  \brief
    get iterator to the beginning of a specific row in a matrix

  \return
    iterator to the beginning of the row, which can be used to iterate over that
    row only
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
ROW_IT Matrix<R, C, T>::row_begin(unsigned int row_num)
{
  return ROW_IT(_rows.at(row_num).begin());
}
/******************************************************************************/
/*!
  \brief
    get iterator to the end of a specific row in a matrix

  \return
    iterator marking the end of a matrix row
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
ROW_IT Matrix<R, C, T>::row_end(unsigned int row_num)
{
  return ROW_IT(_rows.at(row_num).end());
}


/******************************************************************************/
/*!
  \brief
    get iterator to the beginning of a specific column in a matrix

  \return
    iterator to the beginning of the column, which can be used to iterate over
    that column only
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
COL_IT Matrix<R, C, T>::col_begin(unsigned int col_num)
{
  return COL_IT(col_num, _rows.begin());
}
/******************************************************************************/
/*!
  \brief
    get iterator to the end of a specific column in a matrix

  \return
    iterator to the end of a column
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
COL_IT Matrix<R, C, T>::col_end(unsigned int col_num)
{
  return COL_IT(col_num, _rows.begin());
}


/******************************************************************************/
/*!
  \brief
    assignment operator for Matrix

  \param  rhs
    the matrix to set this one equal to

  \return
    reference to this after assignment
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
template<unsigned int A, unsigned int B>
Matrix<R, C, T> &Matrix<R, C, T>::operator=(const Matrix<A, B, T> &rhs)
{
  if(Rows() != rhs.Rows() || Columns() != rhs.Columns() || *this != rhs)
    CopyElemHelper(rhs);

  return *this;
}

/******************************************************************************/
/*!
  \brief
    compound assignment & scalar multiplication operator for Matrix

  \param  rhs
    scalar to multiply by

  \return
    this matrix, with all of its elements scaled by rhs
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T> &Matrix<R, C, T>::operator*=(const T rhs)
{
  for(MTX_IT it = begin(); it != end(); ++it)
  {
    (*it) *= rhs;
  }
  return *this;
}

/******************************************************************************/
/*!
  \brief
    compound assignment & scalar division operator for Matrix

  \param  rhs
    scalar to divide by

  \return
    this matrix, with all of its elements scaled by 1/rhs
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T> &Matrix<R, C, T>::operator/=(const T rhs)
{
  return (*this *= (1/rhs));
}

/******************************************************************************/
/*!
  \brief
    compound assignment & addition operator for Matrix

  \param rhs
    matrix to add to this one

  \return
    reference to this matrix after summing each of its elements with the
    corresponding element of rhs
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T> &Matrix<R, C, T>::operator+=(const Matrix<R, C, T> &rhs)
{
  for(unsigned i = 0; i < R; ++i)
    for(unsigned j = 0; j < C; ++j)
      (*this)(i, j) += rhs(i, j);

  return *this;
}

/******************************************************************************/
/*!
  \brief
    compound assignment & subtraction operator for Matrix

  \param  rhs
    matrix to subtract from this one

  \return
    reference to this matrix after subtracting each element of rhs from the
    corresponding element of this
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T> &Matrix<R, C, T>::operator-=(const Matrix<R, C, T> &rhs)
{
  return (*this += (-rhs));
}


/******************************************************************************/
/*!
  \brief
    elementwise unary negation for matrices

  \return
    a copy of this with each element's sign flipped
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T> Matrix<R, C, T>::operator-(void) const
{
  return (*this * T(-1.0f));
}


/******************************************************************************/
/*!
  \brief
    get number of rows in a matrix (for vectors, this is the dimension)

  \return
    number of rows
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
unsigned int Matrix<R, C, T>::Rows(void)
{
  return R;
}


/******************************************************************************/
/*!
  \brief
    get number of columns in a matrix (for vectors, this will always be 1)

  \return
    number of columns
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
unsigned int Matrix<R, C, T>::Columns(void)
{
  return C;
}


/******************************************************************************/
/*!
  \brief
    fills a matrix with the elements of a 1-dimensional array.

  \param  rhs
    an array of R*C elements of type T
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
void Matrix<R, C, T>::FromArray(T *rhs)
{
  for(unsigned i = 0; i < R*C; ++i)
  {
    (*this)[i] = rhs[i];
  }
}


/******************************************************************************/
/*!
  \brief
    fills a matrix with the elements of a 2-dimensional array.

  \param  rhs
    an array of R arrays of C elements of type T
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
void Matrix<R, C, T>::FromArray(T **rhs)
{
  for(unsigned i = 0; i < R; ++i)
  {
    for(unsigned j = 0; j < C; ++j)
    {
      (*this)(i, j) = rhs[i][j];
    }
  }
}


/******************************************************************************/
/*!
  \brief
    Copies all elements from one matrix into another

  \param  rhs
    the matrix whose elements we want to copy into this one
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
template<unsigned int N, unsigned int M>
void Matrix<R, C, T>::CopyElemHelper(const Matrix<N, M, T> &rhs)
{
  ZeroMtx(*this);

  for(unsigned i = 0; i < std::min(this->Rows(), rhs.Rows()); ++i)
  {
    for(unsigned j = 0; j < std::min(this->Columns(), rhs.Columns()); ++j)
    {
      (*this)(i, j) = rhs(i, j);
    }
  }
}
















////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Matrix nonmember functions start here

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////


/******************************************************************************/
/*!
  \brief
    Matrix equality comparison operator

  \param  lhs
    one of the matrices to compare

  \param rhs
    the other matrix

  \return
    are the 2 matrices equal? That is, is every element of lhs equal to the
    corresponding element of rhs?
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
bool operator==(const Matrix<R, C, T> &lhs, const Matrix<R, C, T> &rhs)
{
  for(unsigned i = 0; i < R; ++i)
  {
    for(unsigned j = 0; j < C; ++j)
    {
      if (abs(lhs(i, j) - rhs(i, j)) > EPSILON)
        return false;
    }
  }
  return true;
}
/******************************************************************************/
/*!
  \brief
    Matrix inequality comparison

  \param  lhs
    one of the matrices to compare

  \param rhs
    the other matrix

  \return
    are the matrices unequal? That is, is there at least one unequal element
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
bool operator!=(const Matrix<R, C, T> &lhs, const Matrix<R, C, T> &rhs)
{
  return !(lhs == rhs);
}


/******************************************************************************/
/*!
  \brief
    Matrix scalar multiplication

  \param  lhs
    the matrix we want to multiply

  \param rhs
    the value to scale by

  \return
    a copy of lhs, with all elements of the copy scaled by rhs
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T> operator*(const Matrix<R, C, T> &lhs, const T &rhs)
{
  Matrix<R, C, T> tmp(lhs);
  tmp *= rhs;
  return tmp;
}
/******************************************************************************/
/*!
  \brief
    Matrix scalar multiplication

  \param lhs
    the value to scale by

  \param  rhs
    the matrix we want to multiply

  \return
    a copy of rhs, with all elements of the copy scaled by lhs
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T> operator*(const T &lhs, const Matrix<R, C, T> &rhs)
{
  return (rhs * lhs);
}
/******************************************************************************/
/*!
  \brief
    Matrix scalar division

  \param  lhs
    the matrix to scale

  \param rhs
    the value to divide by

  \return
    a copy of lhs, with all the copy's elements scaled by 1/rhs
    stuff
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T> operator/(const Matrix<R, C, T> &lhs, const T &rhs)
{
  return (lhs * (1/rhs));
}


/******************************************************************************/
/*!
  \brief
    Matrix addition

  \param  lhs
    one of the matrices to add

  \param rhs
    the other matrix

  \return
    a copy with each element equal to the sum of the corresponding elements in
    lhs and rhs
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T> operator+(const Matrix<R, C, T> &lhs, const Matrix<R, C, T> &rhs)
{
  Matrix<R, C, T> tmp(lhs);
  tmp += rhs;
  return tmp;
}
/******************************************************************************/
/*!
  \brief
    Matrix subtraction

  \param  lhs
    the starting matrix

  \param rhs
    the matrix to subtract from lhs

  \return
    a copy with each element equal to the difference of corresponding elements
    in lhs and rhs
*/
/******************************************************************************/
template<unsigned int R, unsigned int C, typename T>
Matrix<R, C, T> operator-(const Matrix<R, C, T> &lhs, const Matrix<R, C, T> &rhs)
{
  return (lhs + -rhs);
}





















////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Matrix iterator member functions start here

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////



































////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Matrix iterator nonmember functions start here

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

































////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Row iterator member functions start here

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////


































////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Row iterator nonmember functions start here

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////


































////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Column iterator member functions start here

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////


































////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Column iterator nonmember functions start here

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////


























